#define ACTIVATE_COLLIDING_RIGIDBODIES

using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Oops.Xna.Framework.Physics.CollisionSystems;
#if DEBUG
using Oops.Xna.Framework.Viewpoint;
#endif

namespace Oops.Xna.Framework.Physics
{
	// TODO : ACTIVATE_COLLIDING_RIGIDBODIES might cause issues with waking up bodies during a simulation step since it bypasses collision detection.  Might want to turn off.
	/// <summary>
	/// Game component that simulates rigid body physics.
	/// </summary>
	public class PhysicsComponent : AsyncGameComponent, IPhysicsService  
	{
		#region RigidBodyCollection Declaration

		/// <summary>
		/// A collection of <see cref="RigidBody"/> instances.
		/// </summary>
		public class RigidBodyCollection : Collection<RigidBody>
		{
			private PhysicsComponent component;

			/// <summary>
			/// Initializes a new instance of the <see cref="PhysicsComponent.RigidBodyCollection"/> class.
			/// </summary>
			/// <param name="component">The associated <see cref="PhysicsComponent"/>.</param>
			internal RigidBodyCollection(PhysicsComponent component)
			{
				// Hold onto parameters.
				this.component = component;
			}

			protected override void ClearItems()
			{
				for (int i = 0; i < this.Count; i++)
				{
					// Get the current rigid body.
					RigidBody rigidBody = this.Items[i];

					// Remove the removed rigid body's owned collision shapes from the collision system.
					if (this.component.CollisionSystem != null)
					{
						this.component.CollisionSystem.CollisionGroups.Remove(rigidBody.CollisionGroup);
					}

					// Remove the removed rigid body's constraints from the physics service.
					for (int j = rigidBody.Constraints.Count - 1; j >= 0; j--)
					{
						this.component.Constraints.Remove(rigidBody.Constraints[j]);
					}
				}
							
				// Call inherited method.
				base.ClearItems();
			}

			protected override void InsertItem(int index, RigidBody item)
			{
				// Make sure the rigid body has not been added yet.
				if (this.Contains(item))
				{
					throw new ArgumentException("item"); 
				}

				// Add the inserted rigid body's collision shapes to the collision system.
				if (this.component.CollisionSystem != null)
				{
					this.component.CollisionSystem.CollisionGroups.Add(item.CollisionGroup);
				}

				// Call inherited method.
				base.InsertItem(index, item);
			}

			protected override void RemoveItem(int index)
			{
				// Get the rigid body.
				RigidBody item = this.Items[index];

				//// Re-order for memory efficiency. 
				//RigidBody temp = this.Items[this.Count - 1];
				//this.Items[this.Count - 1] = item;
				//this.Items[index] = temp;

				// Remove the removed rigid body's collision shapes from the collision system.
				if (this.component.CollisionSystem != null)
				{
					this.component.CollisionSystem.CollisionGroups.Remove(item.CollisionGroup);
				}

				// Remove the removed rigid body's constraints from the physics service.
				for (int i = item.Constraints.Count - 1; i >= 0; i--)
				{
					this.component.Constraints.Remove(item.Constraints[i]);
				}

				// Call inherited method.
				//base.RemoveItem(this.Count - 1);
				base.RemoveItem(index);
			}

			protected override void SetItem(int index, RigidBody item)
			{
				throw new NotSupportedException();
			}
		}

		#endregion

		#region ConstraintCollection Declaration

		/// <summary>
		/// A collection of <see cref="Constraint"/> instances.
		/// </summary>
		public class ConstraintCollection : Collection<Constraint>
		{
			/// <summary>
			/// Initializes a new instance of the <see cref="PhysicsComponent.ConstraintCollection"/> class.
			/// </summary>
			internal ConstraintCollection() { }

			protected override void ClearItems()
			{
				// Notify constraints of being disabled.
				for (int i = 0; i < this.Count; i++)
				{
					this.Items[i].OnRemoved();
				}

				// Call inherited method.
				base.ClearItems();
			}

			protected override void InsertItem(int index, Constraint item)
			{
				// Make sure the constraint has not been added yet.
				if (this.Contains(item))
				{
					throw new ArgumentException("item");
				}

				// Call inherited method.
				base.InsertItem(index, item);

				// Notify constraint of being enabled.
				item.OnAdded();
			}

			protected override void RemoveItem(int index)
			{
				// Notify constraint of being disabled.
				Constraint item = this.Items[index];
				item.OnRemoved();

				//// Re-order for memory efficiency. 
				//Constraint temp = this.Items[this.Count - 1];
				//this.Items[this.Count - 1] = item;
				//this.Items[index] = temp;

				// Call inherited method.
				base.RemoveItem(index);
				//base.RemoveItem(this.Count - 1);
			}

			protected override void SetItem(int index, Constraint item)
			{
				throw new NotSupportedException();
			}
		}

		#endregion

		#region PhysicsListenerCollection Declaration

		/// <summary>
		/// A collection of <see cref="IPhysicsListener"/> instances.
		/// </summary>
		public class PhysicsListenerCollection : Collection<IPhysicsListener>
		{
			/// <summary>
			/// Initializes a new instance of the <see cref="PhysicsComponent.PhysicsListenerCollection"/> class.
			/// </summary>
			internal PhysicsListenerCollection() { }

			protected override void InsertItem(int index, IPhysicsListener item)
			{
				// Make sure the listener has not been added yet.
				if (this.Contains(item))
				{
					throw new ArgumentException("item");
				}

				// Call inherited method.
				base.InsertItem(index, item);
			}

			protected override void RemoveItem(int index)
			{
				//// Re-order for memory efficiency. 
				//IPhysicsListener temp = this.Items[this.Count - 1];
				//this.Items[this.Count - 1] = item;
				//this.Items[index] = temp;

				// Call inherited method.
				base.RemoveItem(index);
				//base.RemoveItem(this.Count - 1);
			}

			protected override void SetItem(int index, IPhysicsListener item)
			{
				throw new NotSupportedException();
			}
		}

		#endregion

		private CollisionSystem collisionSystem;

		private RigidBodyCollection rigidBodies;
		private ConstraintCollection constraints = new ConstraintCollection();
		private PhysicsListenerCollection listeners = new PhysicsListenerCollection();

		private List<RigidBody> activeRigidBodies = new List<RigidBody>(256);
		
		private float targetStepTime;
		private float maximumStepTime = float.MaxValue;

		private CollisionContext collisionContext;
#if DEBUG
		private IViewpointService viewpointService;

		private BasicEffect effect;
		private VertexDeclaration vertexDeclaration;
		private VertexPositionColor[] vertices = new VertexPositionColor[0];
		private int vertexCount;
#endif
		/// <summary>
		/// Initialize a new instance of the <see cref="PhysicsComponent"/> class.
		/// </summary>
		/// <param name="game">The associated <see cref="Microsoft.Xna.Framework.Game"/>.</param>
		public PhysicsComponent(Game game) : this(game, new SweepAndPruneCollisionSystem(), null) { }

		/// <summary>
		/// Initialize a new instance of the <see cref="PhysicsComponent"/> class.
		/// </summary>
		/// <param name="game">The associated <see cref="Microsoft.Xna.Framework.Game"/>.</param>
		/// <param name="callback">The callback used of collision notification.</param>
		public PhysicsComponent(Game game, ICollisionCallback callback) : this(game, new SweepAndPruneCollisionSystem(), callback) { }

		/// <summary>
		/// Initialize a new instance of the <see cref="PhysicsComponent"/> class.
		/// </summary>
		/// <param name="game">The associated <see cref="Microsoft.Xna.Framework.Game"/>.</param>
		/// <param name="collisionSystem">The associated <see cref="CollisionSystem"/>.</param>
		public PhysicsComponent(Game game, CollisionSystem collisionSystem) : this(game, collisionSystem, null) { }

		/// <summary>
		/// Initialize a new instance of the <see cref="PhysicsComponent"/> class.
		/// </summary>
		/// <param name="game">The associated <see cref="Microsoft.Xna.Framework.Game"/>.</param>
		/// <param name="collisionSystem">The associated <see cref="CollisionSystem"/>.</param>
		/// <param name="callback">The callback used of collision notification.</param>
		public PhysicsComponent(Game game, CollisionSystem collisionSystem, ICollisionCallback callback) : base(game)
		{
			// The collision system must be specified.
			if (collisionSystem == null)
			{
				throw new ArgumentNullException("collisionSystem");
			}

			// Hold onto parameters.
			this.collisionSystem = collisionSystem;

			// Create the rigid bodies collection.
			this.rigidBodies = new RigidBodyCollection(this);

			// Create the collision context.
			this.collisionContext = new CollisionContext(callback);

			// Default the target step time.
			this.targetStepTime = this.Game.IsFixedTimeStep ? (float)this.Game.TargetElapsedTime.TotalSeconds : 1.0f / 60.0f;
			
			// Default property values.
			this.CollisionIterationCount = 1;
			this.ContactIterationCount = 4;
			this.Speed = 1.0f;
		}

		/// <summary>
		/// Gets the associated <see cref="CollisionSystem"/> used to handle broad-phase collision detection.
		/// </summary>
		public CollisionSystem CollisionSystem
		{
			get { return this.collisionSystem; }
		}

		/// <summary>
		/// Gets the collection of <see cref="RigidBody"/> instances registered with the <see cref="PhysicsComponent"/>.
		/// </summary>
		public RigidBodyCollection RigidBodies
		{
			get { return this.rigidBodies; }
		}

		/// <summary>
		/// Gets the collection of <see cref="Constraint"/> instances registered with the <see cref="PhysicsComponent"/>.
		/// </summary>
		public ConstraintCollection Constraints
		{
			get { return this.constraints; }
		}

		/// <summary>
		/// Gets the collection of <see cref="IPhysicsListener"/> instances registered with the <see cref="PhysicsComponent"/>.
		/// </summary>
		public PhysicsListenerCollection Listeners
		{
			get { return this.listeners; }
		}

		/// <summary>
		/// Gets or sets the number of iterations allowed to resolve collisions.
		/// </summary>
		public int CollisionIterationCount { get; set; }

		/// <summary>
		/// Gets or sets the number of iterations allowed to resolve contacts.
		/// </summary>
		public int ContactIterationCount { get; set; }
		
		/// <summary>
		/// Gets or sets the collision tolerance when two <see cref="CollisionShape"/> instances 
		/// are being compared for collision detection.
		/// </summary>
		public float CollisionTolerance
		{
			get { return this.collisionContext.Tolerance; }
			set { this.collisionContext.Tolerance = value; }
		}

		/// <summary>
		/// Gets or sets the target step time for one simulation step.
		/// </summary>
		public TimeSpan TargetStepTime
		{
			get { return TimeSpan.FromSeconds(this.targetStepTime); }
			set	{ this.targetStepTime = (float)value.TotalSeconds; }
		}

		/// <summary>
		/// Gets or sets the maximum step time for one frame of the simulation.
		/// </summary>
		public TimeSpan MaximumStepTime
		{
			get { return TimeSpan.FromSeconds(this.maximumStepTime); }
			set { this.maximumStepTime = (float)value.TotalSeconds;	}
		}

		/// <summary>
		/// Gets or sets the simulation speed scale of the physics processing.
		/// </summary>
		public float Speed { get; set; }
#if DEBUG
		/// <summary>
		/// Gets or sets whether or not the axis-aligned bounding boxes should be drawn for
		/// all registered <see cref="CollisionShape"/> instances.
		/// </summary>
		public bool DrawAABBs { get; set; }
#endif
		#region IPhysicsService Members

		CollisionSystem IPhysicsService.CollisionSystem
		{
			get { return this.collisionSystem; }
		}

		IList<RigidBody> IPhysicsService.RigidBodies
		{
			get { return this.rigidBodies; }
		}

		IList<Constraint> IPhysicsService.Constaints
		{
			get { return this.constraints; }
		}

		IList<IPhysicsListener> IPhysicsService.Listeners
		{
			get { return this.listeners; }
		}

		IList<IAsyncUpdateCallback> IPhysicsService.Callbacks
		{
			get { return this.Callbacks; }
		}

		#endregion

		#region GameComponent Members

		protected override void Dispose(bool disposing)
		{
			if (disposing)
			{
				lock (this)
				{
					// Remove self from the service container.
					if (this.Game.GetService<IPhysicsService>(false) == this)
					{
						this.Game.RemoveService<IPhysicsService>();
					}

					// Remove all constraints.
					this.Constraints.Clear();
					
					// Remove all listeners.
					this.Listeners.Clear();

					// Remove all rigid bodies.
					this.RigidBodies.Clear();
					this.activeRigidBodies.Clear();
				}
			}

			// Call inherited method.
			base.Dispose(disposing);
		}

		#endregion
#if DEBUG
		#region DrawableGameComponent Members

		protected override void LoadContent()
		{
			// Create the effect.
			this.effect = new BasicEffect(this.GraphicsDevice, null);
			this.effect.AmbientLightColor = Vector3.One;
			this.effect.LightingEnabled = false;
			this.effect.VertexColorEnabled = true;

			// Create the vertex declaration.
			this.vertexDeclaration = new VertexDeclaration(this.GraphicsDevice, VertexPositionColor.VertexElements);

			// Call inherited method.
			base.LoadContent();
		}

		protected override void UnloadContent()
		{
			// Dispose of the effect.
			if (this.effect != null)
			{
				this.effect.Dispose();
			}

			// Dispose of the vertex element.
			if (this.vertexDeclaration != null)
			{
				this.vertexDeclaration.Dispose();
			}

			// Call inherited method.
			base.UnloadContent();
		}

		#endregion
#endif
		#region IGameComponent Members

		public override void Initialize()
		{
			// Add self to the service container.
			this.Game.AddService<IPhysicsService>(this);
#if DEBUG
			// Get the viewpoint service.
			this.viewpointService = this.Game.GetService<IViewpointService>(false);
#endif
			// Call inherited method.
			base.Initialize();
		}

		#endregion

		#region IAsyncUpdateable Members

		public override void Update(AsyncGameTime gameTime)
		{
			// Get the elapsed time in seconds including fractions of a second.  Take into account
			// the simulation speed.
			float elapsed = (float)(gameTime.ElapsedGameTime.TotalSeconds * this.Speed);
			
			// Accumulate the step time and account for the maxmimum.
			float stepTime = elapsed;
			if (stepTime > this.maximumStepTime)
			{
				stepTime = this.maximumStepTime;
			}
			
			// Get the speed adjusted target step time.
			float targetStepTime = this.targetStepTime * this.Speed;
			
			// Make sure simulation keeps up with the frame rate.
			while (stepTime > 0.0f)
			{
				// Get the time step for the current iteration.
				float currentStepTime = Math.Min(stepTime, targetStepTime);
				
				// Notify all listeners of the simulation step being started.
				for (int i = 0; i < this.listeners.Count; i++)
				{
					this.listeners[i].PreStep(currentStepTime);
				}

				// Cache all active rigid bodies and detect their collisions.
				this.StoreActiveRigidBodies();
				this.DetectCollisions(currentStepTime);
#if ACTIVATE_COLLIDING_RIGIDBODIES
				// Activate disabled rigid bodies involved in a collision.
				this.ActivateRigidBodies();				
#endif
				// Attempt to resolve the collisions and constraints.
				this.ResolveCollisions(currentStepTime, this.CollisionIterationCount);

				// Integrate the velocities the active rigid bodies
				for (int i = 0; i < this.activeRigidBodies.Count; i++)
				{
					this.activeRigidBodies[i].IntegrateVelocity(currentStepTime);
				}

				// Remove collision restitution and attempt to resolve the collisions
				// and constraints again.
				this.collisionContext.RemoveRestitution();
				this.ResolveCollisions(currentStepTime, this.ContactIterationCount);

				// Iterate through all active rigid bodies.
				for (int i = 0; i < this.activeRigidBodies.Count; i++)
				{
					// Get the current rigid body.
					RigidBody rigidBody = this.activeRigidBodies[i];

					// TODO : If we try to deactivate all rigid bodies, then the compiler directive ACTIVATE_COLLIDING_RIGIDBODIES is unnecessary.  Look into it.
					// Attempt to deactivate and integrate the position of the current rigid body.
					rigidBody.AttemptDeactivation(currentStepTime);
					// TODO : rigidBody.LimitVelocities(); 
					rigidBody.IntegratePosition(currentStepTime);
				}

				// Notify all listeners of the simulation step being finished.
				for (int i = 0; i < this.listeners.Count; i++)
				{
					this.listeners[i].PostStep(currentStepTime);
				}

				// Update the time step to see if another step is necessary for the update.
				stepTime -= targetStepTime;
			}

			// Clear the forces of all rigid bodies.
			for (int i = 0; i < this.rigidBodies.Count; i++)
			{
				this.rigidBodies[i].ClearForces();
			}
#if DEBUG
			// Update the debug drawing information.
			if (!this.AsyncEnabled)
			{
				this.UpdateDebug();
			}
#endif
		}

		public override void EndUpdate()
		{
			// Call inherited method.
			base.EndUpdate();
#if DEBUG
			// Update the debug drawing information after the asynchronous update has finished.
			if (this.AsyncEnabled)
			{
				this.UpdateDebug();
			}
#endif
		}

		#endregion
#if DEBUG
		private void UpdateDebug()
		{
			if (this.DrawAABBs && this.viewpointService != null)
			{
				int count = 0;
				for (int i = 0; i < this.collisionContext.collisions.Count; i++)
				{
					Collision collision = this.collisionContext.collisions[i];

					for (int j = 0; j < collision.Points.Count; j++)
					{
						count++;
					}
				}

				// Ensure the vertex collection size.
				if (this.vertices.Length < this.collisionSystem.CollisionGroups.Count * 24 * 4 + count * 2)
				{
					Array.Resize<VertexPositionColor>(ref this.vertices, this.collisionSystem.CollisionGroups.Count * 24 * 4 + count * 2);
				}

				// Iterate through all the collision system's collision groups.
				int index = 0;
				for (int i = 0; i < this.collisionSystem.CollisionGroups.Count; i++)
				{
					// Hold onto the current collision group.
					CollisionGroup collisionGroup = this.collisionSystem.CollisionGroups[i];

					// Determine the color of the shapes.
					Color color;
					if (collisionGroup.RigidBody != null)
					{
						color = ((collisionGroup.RigidBody.Flags & RigidBodyFlags.Disabled) == RigidBodyFlags.None) ? Color.Green : Color.Red;
					}
					else
					{
						color = Color.White;
					}

					// Iterate through all of the current collision group's collision shapes.
					for (int j = 0; j < collisionGroup.CollisionShapes.Count; j++)
					{
						Vector3 min = collisionGroup.CollisionShapes[j].AABB.Min;
						Vector3 max = collisionGroup.CollisionShapes[j].AABB.Max;

						this.vertices[index++] = new VertexPositionColor(new Vector3(max.X, min.Y, max.Z), color); 
						this.vertices[index++] = new VertexPositionColor(new Vector3(max.X, min.Y, min.Z), color);
						this.vertices[index++] = new VertexPositionColor(new Vector3(max.X, min.Y, max.Z), color); 
						this.vertices[index++] = new VertexPositionColor(new Vector3(min.X, min.Y, max.Z), color);
						this.vertices[index++] = new VertexPositionColor(new Vector3(min.X, min.Y, max.Z), color); 
						this.vertices[index++] = new VertexPositionColor(new Vector3(min.X, min.Y, min.Z), color);
						this.vertices[index++] = new VertexPositionColor(new Vector3(min.X, min.Y, min.Z), color); 
						this.vertices[index++] = new VertexPositionColor(new Vector3(max.X, min.Y, min.Z), color);
						this.vertices[index++] = new VertexPositionColor(new Vector3(max.X, min.Y, max.Z), color); 
						this.vertices[index++] = new VertexPositionColor(new Vector3(max.X, max.Y, max.Z), color);
						this.vertices[index++] = new VertexPositionColor(new Vector3(min.X, min.Y, max.Z), color); 
						this.vertices[index++] = new VertexPositionColor(new Vector3(min.X, max.Y, max.Z), color);
						this.vertices[index++] = new VertexPositionColor(new Vector3(max.X, min.Y, min.Z), color); 
						this.vertices[index++] = new VertexPositionColor(new Vector3(max.X, max.Y, min.Z), color);
						this.vertices[index++] = new VertexPositionColor(new Vector3(min.X, min.Y, min.Z), color); 
						this.vertices[index++] = new VertexPositionColor(new Vector3(min.X, max.Y, min.Z), color);
						this.vertices[index++] = new VertexPositionColor(new Vector3(max.X, max.Y, max.Z), color); 
						this.vertices[index++] = new VertexPositionColor(new Vector3(max.X, max.Y, min.Z), color);
						this.vertices[index++] = new VertexPositionColor(new Vector3(max.X, max.Y, max.Z), color); 
						this.vertices[index++] = new VertexPositionColor(new Vector3(min.X, max.Y, max.Z), color);
						this.vertices[index++] = new VertexPositionColor(new Vector3(min.X, max.Y, max.Z), color); 
						this.vertices[index++] = new VertexPositionColor(new Vector3(min.X, max.Y, min.Z), color);
						this.vertices[index++] = new VertexPositionColor(new Vector3(min.X, max.Y, min.Z), color); 
						this.vertices[index++] = new VertexPositionColor(new Vector3(max.X, max.Y, min.Z), color);
					}
				}

				// Draw collisions in purple.
				Color collisionColor = Color.Purple;
				for (int i = 0; i < this.collisionContext.collisions.Count; i++)
				{
					Collision collision = this.collisionContext.collisions[i];

					for (int j = 0; j < collision.Points.Count; j++)
					{
						CollisionPoint point = collision.Points[j];

						this.vertices[index++] = new VertexPositionColor(point.Position, collisionColor);
						this.vertices[index++] = new VertexPositionColor(point.Position + collision.Normal, collisionColor);
					}
				}

				this.vertexCount = index;
			}
		}

		#region IDrawable Members

		public override void Draw(GameTime gameTime)
		{
			if (this.DrawAABBs && this.viewpointService != null)
			{
				if (this.vertexCount > 0)
				{
					this.GraphicsDevice.VertexDeclaration = this.vertexDeclaration;

					this.effect.View = this.viewpointService.ActiveCamera.View;
					this.effect.Projection = this.viewpointService.ActiveCamera.Projection;

					this.effect.Begin();
					this.effect.CurrentTechnique.Passes[0].Begin();

					this.GraphicsDevice.DrawUserPrimitives<VertexPositionColor>(
						PrimitiveType.LineList,
						this.vertices,
						0,
						this.vertexCount >> 1);

					this.effect.CurrentTechnique.Passes[0].End();
					this.effect.End();
				}
			}

			// Call inherited method.
			base.Draw(gameTime);
		}

		#endregion
#endif
		/// <summary>
		/// Finds all active <see cref="RigidBody"/> instances.
		/// </summary>
		private void StoreActiveRigidBodies()
		{
			this.activeRigidBodies.Clear();
			for (int i = 0; i < this.rigidBodies.Count; i++)
			{
				RigidBody rigidBody = this.rigidBodies[i];
				if ((rigidBody.Flags & RigidBodyFlags.Disabled) == RigidBodyFlags.None)
				{
					this.activeRigidBodies.Add(rigidBody);
				}
			}
		}
#if ACTIVATE_COLLIDING_RIGIDBODIES
		/// <summary>
		/// Attempts to activate <see cref="RigidBody"/> instances that have been collided with.
		/// </summary>
		private void ActivateRigidBodies()
		{
			for (int i = 0; i < this.rigidBodies.Count; i++)
			{
				RigidBody rigidBody = this.rigidBodies[i];
				if ((rigidBody.Flags & RigidBodyFlags.Disabled) == RigidBodyFlags.Disabled)
				{
					if (rigidBody.CollisionGroup.Collisions.Count > 0)
					{
						//rigidBody.Activate();
						rigidBody.Flags &= ~RigidBodyFlags.Disabled;
						this.activeRigidBodies.Add(rigidBody);
					}
				}
			}
		}
#endif
		/// <summary>
		/// Detect the collisions of the active rigid bodies' collision groups.
		/// </summary>
		/// <param name="elapsed">The amount of elapsed game time since the last update.</param>
		private void DetectCollisions(float elapsed)
		{
			// Recycle the collision instances from the previous simulation.
			this.collisionContext.Reset();

			// Clear all rigid body collision associations.
			for (int i = 0; i < this.rigidBodies.Count; i++)
			{
				this.rigidBodies[i].CollisionGroup.Collisions.Clear();
			}

			// Detect all the collisions.
			this.collisionSystem.DetectCollisions(this.activeRigidBodies, this.collisionContext);

			// Commit all discovered collisions.
			this.collisionContext.Commit();
		}

		/// <summary>
		/// Attempts to resolve all collisions and constraints.
		/// </summary>
		/// <param name="elapsed">The amount of elapsed game time since the last update.</param>
		private void ResolveCollisions(float elapsed, int iterationCount)
		{
			// Prepare the collisions for iterative processing.
			for (int i = 0; i < this.collisionContext.collisions.Count; i++)
			{
				this.collisionContext.collisions[i].PreStep(elapsed);
			}
			
			// Prepare the constraints for iterative processing.
			for (int i = 0; i < this.constraints.Count; i++)
			{
				// Get the current constraint.
				Constraint constraint = this.constraints[i];

				// Skip disabled constraints.
				if ((constraint.Flags & ConstraintFlags.Disabled) == ConstraintFlags.None)
				{
					constraint.PreStep(elapsed);
				}
			}
			
			// Iterate through collisions the specified amount of times.
			for (int i = 0; i < iterationCount; i++)
			{
				// Attempt to resolve all collisions.
				bool found = true;
				for (int j = 0; j < this.collisionContext.collisions.Count; j++)
				{
					// Get the current collision.
					Collision collision = this.collisionContext.collisions[j];

					if (!collision.satisfied)
					{
						found |= collision.Solve(elapsed);
					}
				}

				// Attempt to solve all active contraints.
				for (int j = 0; j < this.constraints.Count; j++)
				{
					// Get the current constraint.
					Constraint constraint = this.constraints[j];

					// Solve only active constraints.
					if (!constraint.satisfied && (constraint.Flags & ConstraintFlags.Disabled) == ConstraintFlags.None)
					{
						found |= constraint.Solve(elapsed);
					}
				}

				// Early out, if all collisions are already satisfied.
				if (!found)
				{
				    break;
				}
			}
		}
	}
}
